#include "Matrix2.h"
#include <string.h>

namespace Core
{
	rtMatrix2 rtMatrix2::mat2_zero(rtVector2(0, 0), rtVector2(0, 0));
	rtMatrix2 rtMatrix2::mat2_identity(rtVector2(1, 0), rtVector2(0, 1));

	inline rtMatrix2::rtMatrix2(void) 
	{
	}

	inline rtMatrix2::rtMatrix2(const rtVector2& x, const rtVector2& y) 
	{
		m_Mat[0].x = x.x; m_Mat[0].y = x.y;
		m_Mat[1].x = y.x; m_Mat[1].y = y.y;
	}

	inline rtMatrix2::rtMatrix2(const float xx, const float xy, const float yx, const float yy) 
	{
		m_Mat[0].x = xx; m_Mat[0].y = xy;
		m_Mat[1].x = yx; m_Mat[1].y = yy;
	}

	inline rtMatrix2::rtMatrix2(const float src[2][2]) 
	{
		memcpy(m_Mat, src, 2 * 2 * sizeof(float));
	}

	inline const rtVector2& rtMatrix2::operator [] (int index) const 
	{
		return m_Mat[index];
	}

	inline rtVector2 &rtMatrix2::operator [] (int index) 
	{
		return m_Mat[index];
	}

	inline rtMatrix2 rtMatrix2::operator - () const 
	{
		return rtMatrix2(-m_Mat[0][0], -m_Mat[0][1],
			-m_Mat[1][0], -m_Mat[1][1]);
	}

	inline rtVector2 rtMatrix2::operator * (const rtVector2& vec) const 
	{
		return rtVector2(
			m_Mat[0].x * vec.x + m_Mat[0].y * vec.y,
			m_Mat[1].x * vec.x + m_Mat[1].y * vec.y);
	}

	inline rtMatrix2 rtMatrix2::operator * (const rtMatrix2& a) const 
	{
		return rtMatrix2(
			m_Mat[0].x * a[0].x + m_Mat[0].y * a[1].x,
			m_Mat[0].x * a[0].y + m_Mat[0].y * a[1].y,
			m_Mat[1].x * a[0].x + m_Mat[1].y * a[1].x,
			m_Mat[1].x * a[0].y + m_Mat[1].y * a[1].y);
	}

	inline rtMatrix2 rtMatrix2::operator*(const float a) const 
	{
		return rtMatrix2(
			m_Mat[0].x * a, m_Mat[0].y * a, 
			m_Mat[1].x * a, m_Mat[1].y * a);
	}

	inline rtMatrix2 rtMatrix2::operator + (const rtMatrix2& a) const 
	{
		return rtMatrix2(
			m_Mat[0].x + a[0].x, m_Mat[0].y + a[0].y, 
			m_Mat[1].x + a[1].x, m_Mat[1].y + a[1].y);
	}

	inline rtMatrix2 rtMatrix2::operator - (const rtMatrix2& a) const 
	{
		return rtMatrix2(
			m_Mat[0].x - a[0].x, m_Mat[0].y - a[0].y,
			m_Mat[1].x - a[1].x, m_Mat[1].y - a[1].y);
	}

	inline rtMatrix2& rtMatrix2::operator *= (const float a) 
	{
		m_Mat[0].x *= a; m_Mat[0].y *= a;
		m_Mat[1].x *= a; m_Mat[1].y *= a;

		return *this;
	}

	inline rtMatrix2& rtMatrix2::operator *= (const rtMatrix2& a) 
	{
		float x, y;
		x = m_Mat[0].x; y = m_Mat[0].y;
		m_Mat[0].x = x * a[0].x + y * a[1].x;
		m_Mat[0].y = x * a[0].y + y * a[1].y;
		x = m_Mat[1].x; y = m_Mat[1].y;
		m_Mat[1].x = x * a[0].x + y * a[1].x;
		m_Mat[1].y = x * a[0].y + y * a[1].y;
		return *this;
	}

	inline rtMatrix2& rtMatrix2::operator += (const rtMatrix2& a) 
	{
		m_Mat[0].x += a[0].x; m_Mat[0].y += a[0].y;
		m_Mat[1].x += a[1].x; m_Mat[1].y += a[1].y;

		return *this;
	}

	inline rtMatrix2& rtMatrix2::operator -= (const rtMatrix2& a) 
	{
		m_Mat[0].x -= a[0].x; m_Mat[0].y -= a[0].y;
		m_Mat[1].x -= a[1].x; m_Mat[1].y -= a[1].y;

		return *this;
	}

	inline rtVector2 operator * (const rtVector2& vec, const rtMatrix2& mat2) 
	{
		return mat2 * vec;
	}

	inline rtMatrix2 operator * (const float a, rtMatrix2 const& mat2) 
	{
		return mat2 * a;
	}

	inline rtVector2 &operator *= (rtVector2& vec, const rtMatrix2& mat2) 
	{
		vec = mat2 * vec;
		return vec;
	}

	inline bool rtMatrix2::compare(const rtMatrix2& a) const 
	{
		if (m_Mat[0].compare(a[0]) &&
			m_Mat[1].compare(a[1])) 
		{
			return true;
		}
		return false;
	}

	inline bool rtMatrix2::compare(const rtMatrix2& a, const float epsilon) const 
	{
		if (m_Mat[0].compare(a[0], epsilon) &&
			m_Mat[1].compare(a[1], epsilon)) 
		{
			return true;
		}
		return false;
	}

	inline bool rtMatrix2::operator == (const rtMatrix2& a) const 
	{
		return compare(a);
	}

	inline bool rtMatrix2::operator != (const rtMatrix2& a) const 
	{
		return !compare(a);
	}

	inline void rtMatrix2::zero(void) 
	{
		m_Mat[0].zero();
		m_Mat[1].zero();
	}

	inline void rtMatrix2::identity(void) 
	{
		*this = mat2_identity;
	}

	inline bool rtMatrix2::isIdentity(const float epsilon) const 
	{
		return compare(mat2_identity, epsilon);
	}

	inline bool rtMatrix2::isSymmetric(const float epsilon) const 
	{
		return (rtMath::Fabs(m_Mat[0][1] - m_Mat[1][0]) < epsilon);
	}

	inline bool rtMatrix2::isDiagonal(const float epsilon) const 
	{
		if (rtMath::Fabs(m_Mat[0][1]) > epsilon ||
			rtMath::Fabs(m_Mat[1][0]) > epsilon) 
		{
			return false;
		}
		return true;
	}

	inline float rtMatrix2::trace(void) const 
	{
		return (m_Mat[0][0] + m_Mat[1][1]);
	}

	inline float rtMatrix2::determinant(void) const 
	{
		return m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];
	}

	inline rtMatrix2 rtMatrix2::transpose(void) const 
	{
		return rtMatrix2(m_Mat[0][0], m_Mat[1][0],
			m_Mat[0][1], m_Mat[1][1]);
	}

	inline rtMatrix2 &rtMatrix2::transposeSelf(void) 
	{
		float tmp;

		tmp = m_Mat[0][1];
		m_Mat[0][1] = m_Mat[1][0];
		m_Mat[1][0] = tmp;

		return *this;
	}

	inline rtMatrix2 rtMatrix2::inverse(void) const 
	{
		rtMatrix2 invMat;

		invMat = *this;
		int r = invMat.inverseSelf();
		assert(r);
		return invMat;
	}

	inline rtMatrix2 rtMatrix2::inverseFast(void) const 
	{
		rtMatrix2 invMat;

		invMat = *this;
		int r = invMat.inverseFastSelf();
		assert(r);
		return invMat;
	}

	inline int rtMatrix2::getDimension(void) const 
	{
		return 4;
	}

	inline const float* rtMatrix2::toFloatPtr(void) const 
	{
		return m_Mat[0].toFloatPtr();
	}

	inline float* rtMatrix2::toFloatPtr(void) 
	{
		return m_Mat[0].toFloatPtr();
	}

	bool rtMatrix2::inverseSelf(void) 
	{
		double det, invDet, a;

		det = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		a = m_Mat[0][0];
		m_Mat[0][0] =   m_Mat[1][1] * invDet;
		m_Mat[0][1] = - m_Mat[0][1] * invDet;
		m_Mat[1][0] = - m_Mat[1][0] * invDet;
		m_Mat[1][1] =   a * invDet;

		return true;
	}


	bool rtMatrix2::inverseFastSelf(void)
	{

		double det, invDet, a;

		det = m_Mat[0][0] * m_Mat[1][1] - m_Mat[0][1] * m_Mat[1][0];

		if (rtMath::Fabs(det) < MATRIX_INVERSE_EPSILON) 
		{
			return false;
		}

		invDet = 1.0f / det;

		a = m_Mat[0][0];
		m_Mat[0][0] =   m_Mat[1][1] * invDet;
		m_Mat[0][1] = - m_Mat[0][1] * invDet;
		m_Mat[1][0] = - m_Mat[1][0] * invDet;
		m_Mat[1][1] =   a * invDet;

		return true;
	}

}
